//=========================================================================
// Copyright 2013 Kitware, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//=========================================================================

#ifndef VTKVELODYNEHDLPOSITIONINTERPRETER_H
#define VTKVELODYNEHDLPOSITIONINTERPRETER_H

#include "vtkPositionOrientationPacketInterpreter.h"
#include "GPSProjectionUtils.h"

#include <vtkDoubleArray.h>
#include <vtkIdList.h>
#include <vtkNew.h>
#include <vtkPolyLine.h>

class vtkTransform;
class vtkCustomTransformInterpolator;

//------------------------------------------------------------------------------
class VTK_EXPORT vtkVelodyneHDLPositionPacketInterpreter : public vtkPositionOrientationPacketInterpreter
{
public:
  static vtkVelodyneHDLPositionPacketInterpreter* New();

  vtkTypeMacro(vtkVelodyneHDLPositionPacketInterpreter, vtkPositionOrientationPacketInterpreter)

  vtkVelodyneHDLPositionPacketInterpreter();

  bool HasRawInformation() override { return true;}
  bool HasPositionOrientationInformation() override { return true;}

  bool IsValidPacket(unsigned char const * /*packetData*/, unsigned int dataLength) override;

  void ProcessPacket(unsigned char const * packetData, unsigned int /*dataLength*/) override;

  void InitArrays() override;

  // field names starts with PPS_ because accessed from python wrapping which
  // does not scope using the name of the enum
  // Warning: PPS_LOCKED does not mean that Lidar is synchronized with GPS:
  // see documentation "Webserver User Guide (VLP-16 & HDL-32E)".
  enum PPSState { PPS_ABSENT = 0, PPS_ATTEMPTING_TO_SYNC, PPS_LOCKED, PPS_ERROR };
  vtkGetMacro(LastPPSState, PPSState)
  vtkGetMacro(PPSSynced, bool)
  vtkGetMacro(HasTimeshiftEstimation, bool)
  vtkGetMacro(AssumedHardwareLag, double)
  vtkSetMacro(AssumedHardwareLag, double)

  std::string GetTimeSyncInfo();
  double GetTimeshiftEstimation();
  vtkCustomTransformInterpolator* GetInterpolator();

  vtkSetMacro(ShouldWarnOnWeirdGPSData, bool)

  // Default is false (disabled)
  // If disabled, only GPRMC sentences will be used, they do not provide altitude
  // so z = 0 is used.
  // If enabled, GPRMC sentences will be ignored and GPGGA sentences will be
  // used.
  // If available, the altitude used will be the height above the ellipsoid,
  // because that is what was used as datum when projecting.
  // (could be changed to height above geoid).
  vtkSetMacro(UseGPGGASentences, bool)

  // This function is called in the Position Orientation Reader
  // This function can be implemented by the specific Position Interpreter, to create and fill in
  // the interpreter interpolator, by parsing the raw PositionOrientation information into a
  // proper 6D Euclidean tranform vtkTransform for each received timestep
  // When interpolator is filled in and applied, Tests are testing both
  // Reading, Parsing, and Interpolation (time-based 6D transform interpolation between timesteps)
  void FillInterpolatorFromPositionOrientation() override;

private:
  vtkVelodyneHDLPositionPacketInterpreter(const vtkVelodyneHDLPositionPacketInterpreter&) = delete;
  void operator=(const vtkVelodyneHDLPositionPacketInterpreter&) = delete;

  void FillInterpolatorFromGPSAndHeading(
    vtkPoints* points, vtkDataArray* gpsTime, vtkDataArray* times, vtkDataArray* heading);

  double Offset[3];

  UTMProjector utmProjector;
  vtkIdType pointcount;

  bool ShouldWarnOnWeirdGPSData;
  bool UseGPGGASentences;
  bool PPSSynced;
  PPSState LastPPSState;

  bool hasLastGPSUpdateTime;
  double lastGPSUpdateTime;
  double GPSTimeOffset;
  double convertedGPSUpdateTime;
  bool hasLastLidarUpdateTime;
  double lastLidarUpdateTime;
  double lidarTimeOffset;

  double previousConvertedGPSUpdateTime; // negative means "no previous"

  // HasTimeshiftEstimation can be used to tell if the timeshift between GPS UTC time
  // and Lidar ToH time could becomputed.
  bool HasTimeshiftEstimation;
  // If HasTimeshiftEstimation is true, all measurements of the timeshift are
  // available in this vector. Add the timeshift to lidar time ToH time to get
  // gps time UTC (mod 3600).
  std::vector<double> TimeshiftMeasurements;
  // AssumedHardwareLag is used to compute TimeshiftEstimation,
  // when there is no PPS sync, but we have some NMEA messages with valid fixes.
  // AssumedHardwareLag is the sum of:
  // 1) the time it takes the GPS to emit the first NMEA message after a new fix
  // 2) the time it take this NMEA message to travel over serial link to lidar
  // 3) the time it takes the lidar to decode this NMEA message and place it in
  // a position packet.
  // This value depends of hardware, firmware version and maybe temperature.
  // 0.094 s was measured on two private datasets recorded using the same
  // hardware setup. Precision of this measure is approximately 3e-3 seconds.
  double AssumedHardwareLag;

  vtkSmartPointer<vtkCustomTransformInterpolator> Interp;

  // Data Arrays
  vtkSmartPointer<vtkPoints> points;

  vtkSmartPointer<vtkDoubleArray> lats;
  vtkSmartPointer<vtkDoubleArray> lons;
  vtkSmartPointer<vtkDoubleArray> times;
  vtkSmartPointer<vtkDoubleArray> gpsTime;

  vtkSmartPointer<vtkDoubleArray> gyro1;
  vtkSmartPointer<vtkDoubleArray> gyro2;
  vtkSmartPointer<vtkDoubleArray> gyro3;
  vtkSmartPointer<vtkDoubleArray> temp1;
  vtkSmartPointer<vtkDoubleArray> temp2;
  vtkSmartPointer<vtkDoubleArray> temp3;
  vtkSmartPointer<vtkDoubleArray> accel1x;
  vtkSmartPointer<vtkDoubleArray> accel1y;
  vtkSmartPointer<vtkDoubleArray> accel2x;
  vtkSmartPointer<vtkDoubleArray> accel2y;
  vtkSmartPointer<vtkDoubleArray> accel3x;
  vtkSmartPointer<vtkDoubleArray> accel3y;
  vtkSmartPointer<vtkDoubleArray> heading;

};

#endif // VTKVELODYNEHDLPOSITIONINTERPRETER_H
